/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import com.grt192.mechanism.breakaway.GRTBreakawayRobotBase;
import com.grt192.sensor.*;
import com.grt192.actuator.*;
import com.grt192.controller.breakaway.auto.BreakAwayRobotPositioningController;
import com.grt192.controller.breakaway.teleop.BreakawayTeleopDriveController;
import com.grt192.controller.breakaway.teleop.KickerController;
import com.grt192.controller.breakaway.teleop.RecoveryController;
import com.grt192.controller.breakaway.teleop.RollerController;
import com.grt192.core.GRTRobot;
import com.grt192.mechanism.*;
import com.grt192.mechanism.breakaway.Kicker;
import com.grt192.mechanism.breakaway.Recovery;
import com.grt192.mechanism.breakaway.Roller;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class MainRobot extends GRTRobot {

    //Global Controllers
    //Autonomous Controllers
    //Teleop Controllers
    protected BreakawayTeleopDriveController driveControl;
    protected KickerController kickerController;
    protected RecoveryController recoveryController;
    protected RollerController rollerController;
    //Mechanisms
    protected GRTDriverStation driverStation;
    protected GRTBreakawayRobotBase robotbase;
    protected Kicker shooter;
    protected Roller rollers;
    protected Recovery recovery;

    public MainRobot() {
        //Driver station components
        GRTJoystick leftJoyStick = new GRTJoystick(1, 50, "left");
        GRTJoystick rightJoyStick = new GRTJoystick(2, 50, "right");
        GRTJoystick secondaryJoyStick = new GRTJoystick(3, 50, "secondary");

        //digital outputs
        GRTJaguar leftDT1 = new GRTJaguar(9);
        GRTJaguar leftDT2 = new GRTJaguar(3);
        GRTJaguar recovery1 = new GRTJaguar(5);
        GRTJaguar recovery2 = new GRTJaguar(10);
        GRTJaguar kicker = new GRTJaguar(1);
        GRTJaguar roller1 = new GRTJaguar(4);
        GRTJaguar roller2 = new GRTJaguar(2);
        GRTJaguar rightDT1 = new GRTJaguar(6);
        GRTJaguar rightDT2 = new GRTJaguar(8);
        GRTServo kickerGate = new GRTServo(7);

        //analog inputs
        GRTAccelerometer ax1 = new GRTAccelerometer(2, 50, "BaseAccel");
        GRTGyro gy1 = new GRTGyro(1, 15, "BaseGyro");

        //digital inputs
        GRTEncoder leftEn = new GRTEncoder(1, 2, 50, "LeftWheel");
        GRTEncoder rightEn = new GRTEncoder(3, 4, 50, "RightWheel");
        GRTSwitch kickerSwitch = new GRTSwitch(5, 50, "KickLimit");
        GRTSwitch recoveryUpSwitch = new GRTSwitch(6, 50, "");
        GRTSwitch recoveryGroundSwitch = new GRTSwitch(7, 50, "");

        //Mechanisms
        robotbase = new GRTBreakawayRobotBase(leftDT1, leftDT2, rightDT1, rightDT2,
                ax1, gy1,
                leftEn, rightEn);
        driverStation = new GRTDriverStation(leftJoyStick,
                rightJoyStick,
                secondaryJoyStick);
        shooter = new Kicker(kicker, kickerGate, kickerSwitch);
        rollers = new Roller(roller1, roller2,
                new GRTMaxBotixSonar(3, 50, "leftSonar"),
                new GRTMaxBotixSonar(4, 50, "rightSonar"),
                new GRTMaxBotixSonar(5, 50, "centerSonar"));
        recovery = new Recovery(recovery1, recovery2,
                recoveryUpSwitch, recoveryGroundSwitch,
                new GRTAccelerometer(6, 50, ""));
        //Controllers
        driveControl = new BreakawayTeleopDriveController(robotbase, driverStation);
        kickerController = new KickerController(driverStation, shooter);
        rollerController = new RollerController(driverStation, rollers);
        recoveryController = new RecoveryController(driverStation, recovery);
        teleopControllers.addElement(driveControl);
        teleopControllers.addElement(kickerController);
        teleopControllers.addElement(recoveryController);
        teleopControllers.addElement(rollerController);
        


        this.getWatchdog().setEnabled(false);
        System.out.println("Robot initialized OK");
    }
}
